
#include "Entity.h"
#include "Element.h"
#include "Disc.h"
#include "ConvexPolygon.h"
#include "World.h"

namespace p2 {

IMPLEMENT_RTT(p2, Entity)

Entity::~Entity()
{
}

Entity* Entity::create(Vector2<real> position, real rotation, void* userData)
{
	Entity* newEntity = _allocator.allocate(1);
	if (newEntity) {
		_allocator.construct(newEntity, Entity(position, rotation, userData));
	}
	return newEntity;
}

Entity* Entity::create(void* userData)
{
	Entity* newEntity = _allocator.allocate(1);
	if (newEntity) {
		_allocator.construct(newEntity, Entity(userData));
	}
	return newEntity;
}

bool Entity::initialise(real64 dT)
{
	if (_flags & ENTITY_INITIALISED) {
		return true;
	}

	updateTransform();

	real totalMass = 0.f, totalInertia = 0.f;
	_centerOfMass = Vector2<real>::ZERO;
	Element::Set::iterator iter = _elements.begin();
	for (; iter != _elements.end(); ++iter) {
		ASSERTLOG((*iter)->updateShapeData(), "Entity::initialise");
		if ( !(*iter)->updateShapeData() ) {
			return false;
		}
		(*iter)->updateWorldSpace();
		(*iter)->updateMotionBounds(dT);
		/*
		 * tell broadphase about this element
		 */
		(*iter)->setSapObjectId(_owner->getBroadPhase()->addObject((*iter)->getMotionBounds(), (*iter)->getPlane(), (*iter)));

		/* REF: http://hyperphysics.phy-astr.gsu.edu/hbase/cm.html
		 * "The concept of the center of mass is that of an average of the
		 * masses factored by their distances from a reference point."
		 */
		totalMass += (*iter)->getMass();
		Vector2<real> const& elemLocalCentreOfMass = (*iter)->getPosition() + (*iter)->getCenterOfMass();
		_centerOfMass += (*iter)->getMass() * elemLocalCentreOfMass;
		/*
		 * REF: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=3702: 'bone'
		 * "It is in fact possible to calculate the inertia tensor from two bodies that are joined together rigidly. ..
		 * Much like the mass, it [inertia tensor] contains the sum of the two bodies' inertia tensors.
		 * But then you need to add in the two masses at their respective offsets from the new CG."
		 */
		totalInertia += (*iter)->getInertia();
		/*
		 * "..tensor[i][j] += (mass1*((||offset1||^2*kron) - (offset1[i]*offset1[j])))
		 * where kron is the Kronecker delta, equals 1 if i==j, and 0 if i!=j"
		 */
		totalInertia += (*iter)->getMass() * elemLocalCentreOfMass.squaredLength();
	}
	if (totalMass > 0.f) {
		setMass(totalMass);
		/*
		 * REF: http://hyperphysics.phy-astr.gsu.edu/hbase/cm.html#cmp
		 * => xt = (m1x1 + m2x2 + ... ) / (m1 + m2 + ... )
		 */
		_centerOfMass *= _invMass;
	}
	if (totalInertia > 0.f) {
		/*
		 * REF: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=3702: 'bone'
		 * REF: http://emweb.unl.edu/NEGAHBAN/EM223/note18/note18.htm
		 */
		totalInertia -= _mass * _centerOfMass.squaredLength();
		if (totalInertia > 0.f) {
			setInertia(totalInertia);
		}
	}

	updateTransform();

	_flags |= ENTITY_INITIALISED;
	return true;
}

void Entity::updateTransform()
{
	_transform.setAngle(_a);
	_localOrigin = _centerOfMass * _transform.getRotation();
	/*
	 * shift the center of mass, and therefore the center of rotation and
	 * world origin for all child to _x
	 */
	_transform.setTranslation(_x - _localOrigin);
	_transform.setScale(Vector2<real>::ONE);
}

Transform2<real> Entity::getFutureWorldTransform(real64 dT)
{
	/*
	 * find the current transform for t = th for motion bounds computation
	 */
	Vector2<real> xth(_x + _v * (real) dT);
	real ath(_a + _w * (real) dT);
	Matrix2<real> m(ath);
	return Transform2<real>(xth - _centerOfMass * m, ath, _scale);
}

void Entity::updateMotionBounds(real64 dT)
{
	Element::Set::iterator iter = _elements.begin();
	for (; iter != _elements.end(); ++iter) {
		(*iter)->updateMotionBounds(dT);
	}
}

void Entity::updateWorldSpace()
{
	updateTransform();

	Element::Set::iterator iter = _elements.begin();
	for (; iter != _elements.end(); ++iter) {
		(*iter)->updateWorldSpace();
	}
}

bool Entity::contains(Vector2<real> point, Element*& element)
{
	Element::Set::iterator iter = _elements.begin();
	for (; iter != _elements.end(); ++iter) {
		if ((*iter)->contains(point)) {
			element = (*iter);
			return true;
		}
	}
	return false;
}

void Entity::updateBroadphaseObjects(real64 dT)
{
	UNUSED(dT);
	Element::Set::iterator iter = _elements.begin();
	for (; iter != _elements.end(); ++iter) {
        _owner->getBroadPhase()->updateObject((*iter)->getMotionBounds(), (*iter)->getSapObjectId());
	}
}

bool Entity::attach(Element* element)
{
	if (_elements.find(element) == _elements.end()) {
		element->setOwner(this);
		_elements.insert(element);
		_flags &= ~ENTITY_INITIALISED;
		return true;
	}
	return false;
}

bool Entity::detach(Element* element)
{
	if (_elements.erase(element) == 1) {
		_flags &= ~ENTITY_INITIALISED;
		return true;
	}
	return false;
}

void Entity::integrateVelocity(real64 dT)
{
	if (getInvMass() == 0.f) {
		return;
	}
	/*
	 * angular
	 */
	_w += ((_torque * getInvInertia()) * (real) dT);
    _w = Math<real>::Clamp(_w, -MAX_ANGULAR_VELOCITY, +MAX_ANGULAR_VELOCITY);
	/*
	 * linear
	 */
	_v += ((_force * getInvMass()) * (real) dT);
    _v = Vector2<real>::clamp(_v, +MAX_LINEAR_VELOCITY);

#pragma message WARN("TODO: implement bit damping")

#pragma message WARN("TODO: implement sleeping elements based on kinetic energy store")
	updateKineticEnergy();

	clearForces();
}

void Entity::integratePosition(real64 dT)
{
	if (getInvMass() == 0.f) {
		return;
	}
	/*
	 * update positions
	 */
	_x += (_v * (real) dT);
	_a += (_w * (real) dT);
}

}
